# -*- coding: utf-8 -*-

# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
import collections
import glob
import math
import os
import sys
import time
import weakref
import pyproj
import carla
from std_msgs.msg import String
import socket

from datetime import datetime, timedelta
import rospy
import pynmea2

# Connect to client
client = carla.Client('192.168.3.157', 2000)
client.set_timeout(2.0)

# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()


# 获取本车actor（全城都开奥迪，只本车开特斯拉）
def get_my_car():
    for actor in actors:
        if actor.type_id == 'vehicle.tesla.model3':
            return actor
    return 0


if __name__ == '__main__':

    address = ('192.168.3.78', 31501)  # 服务端地址和端口
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        
    car = get_my_car()

    while(1):     
        c = car.get_control()

        c.throttle  #油门百分比 (0 ~ 1)
        c.steer     #左转，右转百分比（-1 ~ 1）
        c.brake     #刹车百分比（0 ~ 1）
        c.Reverse   #是否倒车（0或1）
        c.gear      #挡位 -1: 'R', 0: 'N'， 1：1
        
        time.sleep(0.5)

        print(c.throttle)

        #速度 km/h
        v = car.get_velocity()    
        speed = 3.6 * math.sqrt(v.x ** 2 + v.y ** 2 + v.z ** 2)


        if c.brake > 0:   # stop
            trigger = "stop"
            s.sendto(trigger.encode(), address)
            time.sleep(0.5)
            print(trigger)

        if c.steer > 0:    # left
            trigger = "left"
            time.sleep(0.5)
            s.sendto(trigger.encode(), address)
            print(trigger)

        if c.steer < 0:    # right
            trigger = "right"
            time.sleep(0.5)
            s.sendto(trigger.encode(), address)
            print(trigger)

        if car.get_control().gear < 0 and speed > 0:    # back
            trigger = "back"
            time.sleep(0.5)
            s.sendto(trigger.encode(), address)
            print(trigger)

        if car.get_control().gear > 0 and speed > 0:  # go straight
            trigger = "up"
            time.sleep(0.5)
            s.sendto(trigger.encode(), address)
            print(trigger)
            
            
        
        







